
//gcc -Wall -o test main.c -lwiringPi
#include <unistd.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <wiringPi.h>
#include <wiringSerial.h>
#include <termios.h>
struct termios options ;
unsigned char UartBuff[11];
int counter=0;
float a[3],g[3],yaw,pitch,roll,h[3],pas,height;

void getuart(int fd)
{
        while (fd>=0)
        {
            UartBuff[counter]=serialGetchar(fd);
            if (counter==0 && UartBuff[0]!=0x55) return;
            counter++;
            if (counter==11)
            {
                counter=0;
		return;
            }
        }
}


int main(void)
{
    int fd;
    if((fd = serialOpen ("/dev/ttyUSB2",9600))<0)
    {
        printf("serial err\n");
    }
    while(1)
    {
	getuart(fd);
        if(UartBuff[0]==0x55)
        {
	    //printf("%d",UartBuff[0]);
	    switch (UartBuff[1]){
		case 0x51:
		    a[0]=(short)((short)(UartBuff[3])<<8 | UartBuff[2])/32768.0*16;
  		    a[1]=(short)((short)(UartBuff[5])<<8 | UartBuff[4])/32768.0*16;
		    a[2]=(short)((short)(UartBuff[7])<<8 | UartBuff[6])/32768.0*16;
		    //printf("accel:%4.3f,%4.3f,%4.3f\n",a[0],a[1],a[2]);
		    break;
		case 0x52:
		    g[0]=(short)((short)(UartBuff[3])<<8 | UartBuff[2])/32768.0*2000;
                    g[1]=(short)((short)(UartBuff[5])<<8 | UartBuff[4])/32768.0*2000;
                    g[2]=(short)((short)(UartBuff[7])<<8 | UartBuff[6])/32768.0*2000;
                    //printf("gypro%4.3f,%4.3f,%4.3f\n",g[0],g[1],g[2]);
                    break;
		 case 0x53:
                    roll=(short)((short)(UartBuff[3])<<8 | UartBuff[2])/32768.0*180;
                    pitch=(short)((short)(UartBuff[5])<<8 | UartBuff[4])/32768.0*180;
                    yaw=(short)((short)(UartBuff[7])<<8 | UartBuff[6])/32768.0*180;
                    printf("xyz-rpy:%4.3f,%4.3f,%4.3f\n",roll,pitch,yaw);
                    break;
		case 0x54:
                    h[0]=(short)((short)(UartBuff[3])<<8 | UartBuff[2]);
                    h[1]=(short)((short)(UartBuff[5])<<8 | UartBuff[4]);
                    h[2]=(short)((short)(UartBuff[7])<<8 | UartBuff[6]);
                    printf("h:%4.3f,%4.3f,%4.3f\n",h[0],h[1],h[2]);
                    break;
		case 0x56:
                    pas=(UartBuff[5]<<24 | UartBuff[4]<<16 | UartBuff[3]<<8 | UartBuff[2]);
                    height=(UartBuff[9]<<24 | UartBuff[8]<<16 | UartBuff[7]<<8 | UartBuff[6])/100;
                    printf("pass-height:%4.3fpa,%4.3fm\n",pas,height);
                    break;
		
	    }
	}
    }
    return 0;
}

